#include "power_ctrl.h"

#include "base_drv/drv_conf.h"
#include "base_drv/super_cap.h"
#include "base_drv/referee.h"

#include "pid_parameters.h"

#include <math.h>
#include <string.h>


extern CAN_HandleTypeDef SUPER_CAP_CAN_HANDLE;

pid_struct_t power_limit_pid;
//pid_struct_t power_cap_pid;
pid_struct_t power_limit_pid_Nban;

void power_ctrl_init(chassis_power_lim_t* chassis_power_lim){
	//pid_init(&power_limit_pid, 4.0f,0.25f,0.3f,C620_OUTPUT_MAX/350,C620_OUTPUT_MAX/100,0);
	//pid_init(&power_cap_pid, 4.9f,0.3f,0.0f,C620_OUTPUT_MAX/350,C620_OUTPUT_MAX/100,0);
	memcpy(&power_limit_pid,&power_limit_pid_init_val,sizeof(pid_struct_t));
	//memcpy(&power_cap_pid,&power_cap_pid_init_val,sizeof(pid_struct_t));
	memcpy(&power_limit_pid_Nban,&power_limit_pid_Nban_init_val,sizeof(pid_struct_t));

	//保持以下初始化值等同于不限制功率
	chassis_power_lim->max_total_out = 4.0f*C620_OUTPUT_MAX;
	chassis_power_lim->vxy_output_max = chassis_vx_pid_init_val.out_max;
	chassis_power_lim->wz_output_max = chassis_wz_pid_init_val.out_max;
	chassis_power_lim->vxy_ratio = 1.0f;
	chassis_power_lim->wz_ratio = 1.0f;
}

float pid_limit_low(pid_struct_t* m_pid, float min,float limit_target,float current){
	float limit_output = 1.0f;
	if(current < min){
		limit_output = pid_calc(m_pid, limit_target,current);
	}else{
		if(m_pid->i_out > m_pid->ki){
			m_pid->i_out -= I_RESTORE_RATE * m_pid->ki;
			limit_output = m_pid->i_out;
		}else limit_output= 1;
	}
	return limit_output;
}

void power_ctrl(chassis_power_lim_t* chassis_power_lim,uint16_t max_power)//输出chassis_power_lim
{
	float result_ratio = 1;
	
	if(max_power < 40 || max_power > 150) max_power = 50;//输出最大功率
	if(power_heat_data.chassis_power_buffer == 0){
		result_ratio=1;
	}else{
				result_ratio = pid_calc(&power_limit_pid_Nban, POWER_BUFFER_PEAK,power_heat_data.chassis_power_buffer);
	}
		
	if(result_ratio < 1) 
	{result_ratio = 1;}
	else if(game_robot_status.chassis_power_limit == 65535){result_ratio = 1;}
	chassis_power_lim->vxy_ratio = result_ratio;
}

